// Tonality.cpp: implementation of the Tonality class.
//
//////////////////////////////////////////////////////////////////////

#include <assert.h>
#include <math.h>
#include "Tonality.h"
#include "readwave.h"
#include "resample.h"
#include "ToneProfile.h"

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

Tonality::Tonality()
{
	assert(FRAME_SIZE<=FFT_SIZE);
	_fftTool = new SplitRadixFFT (FFT_ORDER);
	GenHamWindow(FRAME_SIZE);
	_f0s = GetFreqPoints(&_numF0);
	InitFBank(100, 3000);
	AdaptPolyProfile();
}

Tonality::~Tonality()
{
	delete	_f0s;
	delete	_f0Wt;
	delete	_fftTool;
}

void Tonality::GenHamWindow(int frameSize)
{
	int i;
	float a;
    
	a = PI*2 / (frameSize - 1);
	for (i=0; i<frameSize; i++)
		_hamWin[i] = (float)(0.54 - 0.46 * cos(a*i));
}

void Tonality::AdaptPolyProfile()
{
	int	tone, bin;
	int	i;

	// consider chord effects
	// Tonal Description of Music Audio Signal (Page 104)
	for (tone = 0; tone < 2; tone++)
	{
		for (bin = 0; bin < HPCPDim; bin++)
		{
			_keyProf[tone][bin] = 0;
			for (i = 0; i < HPCPDim; i++)
				_keyProf[tone][bin] += TemplyProf[tone][i] * PolyKeyWt[tone][bin][i];
		}
	}
	float	profbak[2][HPCPDim];
	float	pitch, step;
	float	f, nf, wt;
	int		k, b, b1;
	float	tail;
	
	// consider harmonics effect
	memcpy(profbak, _keyProf, sizeof(float)*2*HPCPDim);
	step = (float)pow(2, 1.0F/HPCPDim);
	for (tone = 0; tone < 2; tone++)
	{
		pitch = MinF0;
		for (bin = 0; bin < HPCPDim; bin++)
		{
			wt = 1;
			// skip 2n-th harmonics
			for (k = 3; k < 8; k+=2)
			{				
				f = pitch * k;
				nf = 12 * (float)(log(f/MinF0)/log(2));
				tail = nf - (int)nf;
				b = (int)nf % HPCPDim;
				b1= (b+1) % HPCPDim;
				wt *= hFactor * hFactor;
				_keyProf[tone][bin] += wt * (profbak[tone][b] * tail + profbak[tone][b1] * (1 - tail));
			}
			pitch *= step;
		}
	}
}

bool Tonality::GetAudioTonality(char *wavFile, ToneInfo *toneInfo)
{
	bool	ret;
	float	*hpcp;
	int		numFeat;
	int		tone, i;
	int		shift, j;
	float	tonality;
	float	maxTonalty[2];
	int		maxShift[2];
	float	avgHPCP[HPCPDim];

	ret = GetAudioHPCP(wavFile, &hpcp, &numFeat);
	if (ret==false)
		return	false;

	int validFrame = 0;
	float	sum;
	float	squHPCP;
	float	squSum[2];

	// calculate average of HPCP
	for (j = 0; j < HPCPDim; j++)
		avgHPCP[j] = 0;
	for (i = 0; i < numFeat; i++)
	{
		sum = 0;
		for (j = 0; j < HPCPDim; j++)
		{
			avgHPCP[j] += hpcp[i*HPCPDim+j];
			sum += hpcp[i*HPCPDim+j];
		}
		validFrame += (sum > 1E-6F);
	}

	squHPCP = 0;
	for (j = 0; j < HPCPDim; j++)
	{
		avgHPCP[j] /= validFrame;
		squHPCP += avgHPCP[j] * avgHPCP[j];
	}
	for (tone = 0; tone < 2; tone++)
	{
		squSum[tone] = 0;
		for (j = 0; j < HPCPDim; j++)
			squSum[tone] += _keyProf[tone][j] * _keyProf[tone][j];
	}

	for (tone = 0; tone < 2; tone++)
	{
		maxTonalty[tone] = -1;
		maxShift[tone] = -1;
		for (shift = 0; shift < HPCPDim; shift++)
		{
			float	dotSum;
			int		bin;
			dotSum = 0;
			for (j = 0; j < HPCPDim; j++)
			{
				bin = (j + shift) % HPCPDim;
				dotSum += _keyProf[tone][j] * avgHPCP[bin];
			}
			tonality = dotSum / (float)sqrt(squSum[tone]*squHPCP);
			if (tonality > maxTonalty[tone])
			{
				maxTonalty[tone] = tonality;
				maxShift[tone] = shift;
			}
		}
		assert(maxShift[tone] >= 0);
	}
	toneInfo->major = maxTonalty[0];
	toneInfo->minor = maxTonalty[1];
	if (toneInfo->major > toneInfo->minor)
		toneInfo->shift = maxShift[0];
	else
		toneInfo->shift = maxShift[1];

	for (j = 0; j < HPCPDim; j++)
	{
		toneInfo->THPCP[j] = avgHPCP[(j + toneInfo->shift) % HPCPDim];
	}
	delete	hpcp;
	return	true;
}

bool Tonality::GetAudioHPCP(char *wavFile, float **pHPCP, int *numFeat)
{
	short	*wavData;
	int		numData;
	float	*fftPowers;
	char	*peakBand;
	float	*sumH;
	float	*hpcpFeat;
	float	*rawChroma;	
	int		numFrame;

	wavData = GetWavData(wavFile, &numData);
	if (wavData == NULL || numData < FRAME_SIZE)
		return	false;

	fftPowers = GetFFTPower(wavData, numData, &numFrame);
	delete	wavData;
	if (fftPowers == NULL || numFrame < SM_L)
		return	false;

	_bUtter = DetectAudioHarmonicPart(fftPowers, numFrame);

	// whiten spectrum with envelop
	WhitenSpectrumWithEnvelop(fftPowers, numFrame);

	// set spectrum zero where are not peaks
	peakBand = SelectSpectralPeaks(fftPowers, numFrame);

	// get spectral sum of harmonics
	sumH = CalcSumHarmonics(fftPowers, peakBand, numFrame);

	// sum the energy of F0 to corresponding 4 bins
	rawChroma = SumChromaBins(sumH, numFrame);

	// smooth chroma by the average of 3 middle energy of 5 frames
	SmoothChroma(rawChroma, numFrame);

	// normalize chroma by maximum element
	NormalizeChroma(rawChroma, numFrame);

	// tune the frequency by finding the central bin
	TuneFrequency(rawChroma, numFrame);

	// merge 3 chroma bins to 1 Tonality bin	
	hpcpFeat = Merge3Bin2HPCP(rawChroma, numFrame);

	delete	fftPowers;
	delete	_bUtter;
	delete	peakBand;
	delete	rawChroma;
	delete	sumH;

	*pHPCP = hpcpFeat;
	*numFeat = numFrame;

	return	true;
}

bool* Tonality::DetectAudioHarmonicPart(float *fftPower, int nFrame)
{
	int		frame;
	float	bandEn[BN];
	float	avgEn;
	float	varEn;
	int		ch, i;
	int		span;
	float	*fft;
	float	*energy;
	float	*quotient;
	bool	*bSound;

	energy = new float [nFrame];
	quotient=new float [nFrame];
	bSound = new bool  [nFrame];

	assert(energy && quotient && bSound);

	// get the energy and band energy quotient
	for (frame = 0; frame < nFrame; frame++)
	{
		energy[frame] = 0;
		fft = &fftPower[frame*HALF_FFT];

		avgEn = varEn = 0;
		memset(bandEn, 0, sizeof(float)*BN);
		for (i = 0; i < HALF_FFT; i++)
		{
			ch = _chlMap[i];
			if (ch<0 || ch>=BN)	continue;
			bandEn[ch] += _chlWeight[i] * fft[i];
			energy[frame] += fft[i];
		}
		energy[frame] = (float)log10(energy[frame]+1)*10.0F;
		for (i = 0; i < BN; i++)
		{
			bandEn[i] = (float)log10(bandEn[i]+1)*10.0F;
			avgEn += bandEn[i];
		}
		avgEn /= BN;
		for (i = 0; i < BN; i++)
		{
			float dif;
			dif = bandEn[i] - avgEn;
			varEn += dif*dif;
		}
		varEn /= BN;
		varEn = (float)sqrt(varEn);
		quotient[frame] = varEn;
	}
	// detect harmonic part by energy and quotient
	span = (int)(QSpan / SHIFT_TIME);
	for (frame = 0; frame < nFrame; frame++)
		bSound[frame] = false;

	for (frame = 0; frame < nFrame; frame++)
	{
		if (energy[frame] < 57)
			continue;
		if (quotient[frame] < QThres)
			continue;
		for (i = frame-span; i <=frame+span; i++)
		{
			if (i<0 || i>=nFrame)
				continue;
			bSound[i] = true;
		}
	}
	delete	energy;
	delete	quotient;
	return	bSound;
}

float* Tonality::Merge3Bin2HPCP(float *chroma, int nFrame)
{
	int	frame, bin;
	float *chr, *hp;
	float *hpcp;

	hpcp = new float [HPCPDim*nFrame];
	for (frame = 0; frame < nFrame; frame++)
	{		
		chr = &chroma[frame*ChromaDim];
		hp  = &hpcp[frame*HPCPDim];
		memset(hp, 0, sizeof(float)*HPCPDim);
		if (_bUtter[frame]==false)	continue;
		for (bin = 0; bin < ChromaDim; bin++)
		{
			hp[bin/3] += chr[bin];
		}
	}
	return	hpcp;
}

void Tonality::TuneFrequency(float *chroma, int nFrame)
{
	int		frame, bin;
	float	*chr;
	float	hpcp[ChromaDim];
	float	avgBin[3];
	int		r, h;

	for (bin = 0; bin < 3; bin++)
		avgBin[bin] = 0;

	// sum 3 bins to find central bin
	for (frame = 0; frame < nFrame; frame++)
	{
		if (_bUtter[frame]==false)	continue;
		// set bins zero where are not peaks
		chr = &chroma[frame*ChromaDim];
		memcpy(hpcp, chr, sizeof(float)*ChromaDim);
		for (bin = 0; bin < ChromaDim; bin++)
		{
			r = (bin - 1 + ChromaDim) % ChromaDim;
			h = (bin + 1) % ChromaDim;
			if (hpcp[r] > hpcp[bin] || hpcp[h] > hpcp[bin])
				hpcp[bin] = 0;
		}

		// add peak energy to 3 bins
		for (bin = 0; bin < ChromaDim; bin++)
		{
			avgBin[bin % 3] += hpcp[bin];
		}
	}
	float sortBin[3];

	memcpy(sortBin, avgBin, sizeof(float)*3);
	SortQueue(sortBin, 3);

	// locate the max bin index
	int	maxNo = -1;
	for (bin = 0; bin < 3; bin++)
	{
		if (fabs(avgBin[bin] - sortBin[0]) < 1E-6)
		{
			maxNo = bin;
			break;
		}		
	}
	assert(maxNo >= 0);
	if (maxNo = 1)
		return;

	// shift bins according to central bins
	int step = maxNo - 1;
	int oriIdx;
	for (frame = 0; frame < nFrame; frame++)
	{
		if (_bUtter[frame]==false)	continue;
		chr = &chroma[frame*ChromaDim];
		memcpy(hpcp, chr, sizeof(float)*ChromaDim);
		for (bin = 0; bin < ChromaDim; bin++)
		{
			oriIdx = (bin + step + ChromaDim) % ChromaDim;
			chr[bin] = hpcp[oriIdx];
		}
	}
}

void Tonality::NormalizeChroma(float *chroma, int nFrame)
{
	int frame, bin;
	float *chr;
	float max;

	for (frame = 0; frame < nFrame; frame++)
	{
		if (_bUtter[frame]==false)	continue;
		chr = &chroma[frame*ChromaDim];
		max = -1;
		for (bin = 0; bin < ChromaDim; bin++)
		{
			max = (chr[bin] > max) ? chr[bin] : max;
		}
		assert(max > 0);
		for (bin = 0; bin < ChromaDim; bin++)
			chr[bin] /= max;
	}
}

void Tonality::SmoothChroma(float *chroma, int nFrame)
{
	float	*oriChroma;
	int		frame, i;
	float	chrQueue[SM_L];
	int		bin;
	bool	bSpot;

	oriChroma = new float [ChromaDim*nFrame];
	memcpy(oriChroma, chroma, sizeof(float)*ChromaDim*nFrame);

	for (frame = SM_R; frame < nFrame - SM_R; frame++)
	{
		if (_bUtter[frame]==false)	continue;
		bSpot = false;
		for (i = -SM_R; i <= SM_R; i++)
		{
			if (_bUtter[frame+i]==false)
			{
				bSpot = true;
				break;
			}
		}
		if (bSpot==false)
		{
			// all frame are valid
			for (bin = 0; bin < ChromaDim; bin++)
			{
				for (i = -SM_R; i <= SM_R; i++)
				{
					chrQueue[i+SM_R] = oriChroma[(frame+i)*ChromaDim+bin];
				}
				SortQueue(chrQueue, SM_L);	
				// only use middle data
				float avg = 0;
				for (i = 1; i < SM_L-1; i++)
					avg += chrQueue[i];
				avg /= (SM_L-2);
				chroma[frame*ChromaDim+bin] = avg;
			}
		}
		else
		{
			// part of frame are non-harmonic
			for (bin = 0; bin < ChromaDim; bin++)
			{
				int	cnt=0;
				for (i = -SM_R; i <= SM_R; i++)
				{
					if (_bUtter[frame+i]==false)
						continue;
					chrQueue[cnt++] = oriChroma[(frame+i)*ChromaDim+bin];
				}
				SortQueue(chrQueue, cnt);
				// only use the median data
				chroma[frame*ChromaDim+bin] = chrQueue[cnt/2];
			}
		}
	}
	delete	oriChroma;
}

void Tonality::SortQueue(float *queue, int num)
{
	int i, j;
	float swap;

	for (i = 0; i < num-1; i++)
	{
		for (j = i+1; j < num; j++)
		{
			if (queue[i] < queue[j])
			{
				swap = queue[i];
				queue[i] = queue[j];
				queue[j] = swap;
			}
		}
	}
}

float* Tonality::SumChromaBins(float *sumHarm, int nFrame)
{
	int		frame, j;
	int		bin, i;
	int		idx;
	float	*chroma;
	float	*ch, *shs;

	chroma = new float [ChromaDim * nFrame];
	for (frame = 0; frame < nFrame; frame++)
	{
		ch = &chroma[frame*ChromaDim];
		shs= &sumHarm[frame*_numF0];
		for (bin = 0; bin < ChromaDim; bin++)
			ch[bin] = 0;
		if (_bUtter[frame]==false)	continue;
		for (i = 0; i < _numF0; i++)
		{
			for (j = 0; j < BinCover; j++)
			{
				idx = _f0Wt[i].chrIdx[j];
				ch[idx] += shs[i] * _f0Wt[i].wt[j];
			}
		}
	}
	return	chroma;
}

float* Tonality::GetFreqPoints(int *numF)
{
	float	reso, f;
	int		numFreq;
	float	maxHF;
	float	*freq;
	int		i, j;
	float	fn[ChromaDim];
	
	_bandW = (float)TGT_SAMPLERATE/FFT_SIZE;
	maxHF = (MaxHarmFreq < MinF0*MaxHarmNum) ? MaxHarmFreq : MinF0*MaxHarmNum;
	reso = _bandW / maxHF;
	_fStep = 1 + reso;
	
	numFreq = 0;
	for (f = MinF0; f < MaxF0; f *= _fStep)
		numFreq++;
			
	freq = new float [numFreq];
	for (i = 0, f = MinF0; f < MaxF0; f *= _fStep)
		freq[i++] = f;

	for (j = 0; j < ChromaDim; j++)
	{
		fn[j] = MinF0 * (float)pow(2, j/(float)ChromaDim);
	}

	float	mf;
	float	tail, wt;
	float	d, l, L2;
	int		binCnt;

	_f0Wt = new ChrmWt [numFreq];	

	// calculate the contribution to each bin for each freq
	for (i = 0; i < numFreq; i++)
	{
		binCnt = 0;
		for (j = 0; j < ChromaDim; j++)
		{
			// Tonal Description of Music Audio Signal(Page 97)
			mf = (float)(log(freq[i]/fn[j])/log(2));
			tail = (float)fabs(mf - (int)(mf+0.5F));
			d = tail * 12;
			l = 4 / 3.0F; // semitone
			L2 = l/2;
			if (d > L2)		continue;

			wt = (float)cos(PI/2 * d / L2);
			wt = wt * wt;
			if (binCnt < BinCover)
			{
				int idx = binCnt;
				_f0Wt[i].f = freq[i];
				_f0Wt[i].chrIdx[idx] = j;
				_f0Wt[i].wt[idx] = wt;
				binCnt++;
			}
			else
			{
				for (int k = 0; k < binCnt; k++)
				{
					if (_f0Wt[i].wt[k] < wt)
					{
						_f0Wt[i].f = freq[i];
						_f0Wt[i].chrIdx[k] = j;
						_f0Wt[i].wt[k] = wt;
						break;
					}
				}
			}
		}
		for (j = binCnt; j < BinCover; j++)
		{
			_f0Wt[i].f = freq[i];
			_f0Wt[i].chrIdx[j] = 0;
			_f0Wt[i].wt[j] = 0;
		}
	}

	*numF = numFreq;
	return	freq;
}

float* Tonality::CalcSumHarmonics(float *fftPowers, char *bPeak, int nFrame)
{
	float	*shs, f;
	float	*sumh;
	float	*fft;
	char	*isPeak;
	int		frame;
	int		i, band, k;
	float	factor;
	int		low, high;
	float	avgE;
	float	fftSum[HALF_FFT];

	shs = new float [_numF0 * nFrame];
	for (frame = 0; frame < nFrame; frame++)
	{
		if (_bUtter[frame]==false)	continue;
		fft = &fftPowers[frame*HALF_FFT];
		isPeak=&bPeak[frame*HALF_FFT];
		sumh = &shs[_numF0*frame];

		fftSum[0] = fft[0];
		for (band = 1; band < HALF_FFT; band++)
			fftSum[band] = fftSum[band-1] + fft[band];

		for (i = 0; i < _numF0; i++)
		{
			sumh[i] = 0;
			factor = 1;
			for (k = 1; k < MaxHarmNum; k++)
			{
				f = _f0s[i] * k;
				if (f > MaxHarmFreq)
					break;
				band = (int)(f / _bandW + 0.5F);
				band = (band < HALF_FFT) ? band : HALF_FFT-1;
				if (isPeak[band]==0)
					continue;

				low = (int)((f - _f0s[i] / 2) / _bandW + 0.5F) - 1;
				high =(int)((f + _f0s[i] / 2) / _bandW + 0.5F);
				low  = (low < 1) ? 1 : low;
				high = (high < HALF_FFT) ? high : HALF_FFT-1;
				assert(high-low > 0);
				avgE = (fftSum[high]-fftSum[low])/(high-low);
				if(avgE > 10E-6F)
					sumh[i] += fft[band] / avgE * factor;
				factor *= hFactor;
			}
			assert(sumh[i]>=0);
		}
	}
	return	shs;
}

char* Tonality::SelectSpectralPeaks(float *fftPowers, int nFrame)
{
	int		begBand;
	float	_bandW;
	int		band, i;
	int		frame;
	float	*fft;
	char	*peakBand;
	char	*pos;
	int		bandR;

	_bandW = (float)TGT_SAMPLERATE/FFT_SIZE;
	begBand = (int)(MinF0 / _bandW);
	peakBand = new char [nFrame*HALF_FFT];
	assert(peakBand);

	bandR = (int)(PeakCover/_bandW);
	for (frame = 0; frame < nFrame; frame++)
	{
		if (_bUtter[frame]==false)	continue;
		fft = &fftPowers[frame*HALF_FFT];
		pos = &peakBand[frame*HALF_FFT];
		for (band = 0; band < begBand; band++)
			pos[band] = 0;
		for (band = begBand; band < HALF_FFT; band++)
		{			
			bool bMax = true;
			for (i = band - bandR; i <= band + bandR && i != band && i < HALF_FFT; i++)
			{
				if (fft[i] > fft[band])
				{
					bMax = false;
					break;
				}
			}
			if (bMax == true)
				pos[band] = 1;
			else
				pos[band] = 0;
		}
	}
	return	peakBand;
}

short*	Tonality::GetWavData(char *wavFile, int *numData)
{
	int		numOriSamples;
	int		numNewData;
	short	*oriWaveData;
	short	*waveData;
	WavHead	wavHead;
	int		i;
	
	oriWaveData = ReadAULawPCMWave(wavFile, &numOriSamples, &wavHead);
	
	if (oriWaveData == NULL)
		return NULL;

	if (wavHead.NChannels != 1)
	{
		printf("File channel is not 1 for %s\n", wavFile);
		return	NULL;
	}
	
	// limit maximum sample number to 30 min
	// too long sample may cause memory explosion when down-sampling
	if (numOriSamples/wavHead.SampleRate > 60*30)
		numOriSamples = wavHead.SampleRate*60*30;
	
	// remove the DC of the signal
	int	avg = 0;
	for (i = 0; i < numOriSamples; i++)
		avg += oriWaveData[i];
	avg /= numOriSamples;
	for (i = 0; i < numOriSamples; i++)
		oriWaveData[i] -= avg;
	
	if (wavHead.SampleRate != TGT_SAMPLERATE) 
	{
		waveData = DownSampling(oriWaveData, wavHead.SampleRate, numOriSamples, &numNewData);
		*numData = numNewData;
	}
	else
	{
		waveData = oriWaveData;
		*numData = numOriSamples;
	}
	return	waveData;
}

short* Tonality::DownSampling(short *waveData, int oriSampleRate, int numSample, int *numNewData)
{
	float	downRatio;
	int		newLen;
	short	*downedData;
	
	assert(waveData);
	downRatio = (float) TGT_SAMPLERATE / oriSampleRate;
	newLen = (int)(numSample * downRatio);
	
	downedData = new short [newLen*2];
	assert(downedData);
	int retLen;
	retLen = ResampleBuf(waveData, downedData, numSample, TGT_SAMPLERATE, oriSampleRate, 1);
	assert(retLen < newLen*2);
	newLen = retLen;

	delete waveData;
	
	*numNewData = newLen;
	return	downedData;
}

float* Tonality::GetFFTPower(short *waveData, int numSamples, int *numFrame)
{
	float *fftPower;
	float fftbuf[FFT_SIZE];	
	
	int		validSamples;
	int		nFrame;
	float	*powerFFT;
	int		frame, i;
	float	re ,im;
	short	*wave;
	
	wave = waveData;	
	validSamples = numSamples - FRAME_SIZE;
	nFrame = validSamples / FRAME_SHIFT + 1;
	if (nFrame <= 0)
		return NULL;
	
	powerFFT = new float [nFrame*HALF_FFT];
	assert(powerFFT);
	
	for (frame = 0; frame < nFrame; frame++)
	{
		for (i = 0; i < FRAME_SIZE; i++)
			fftbuf[i] = wave[frame*FRAME_SHIFT+i];
		for (; i < FFT_SIZE; i++)
			fftbuf[i] = 0;
		
		// apply hamming window
		for (i = 0; i < FRAME_SIZE; i++)
			fftbuf[i] *= _hamWin[i];
		
		// apply FFT
		_fftTool->XForm(&fftbuf[0]);
		
		// get the power of fft
		fftPower = &powerFFT[frame*HALF_FFT];
		fftPower[0] = fftbuf[0] * fftbuf[0];
		for (i = 1; i < HALF_FFT; i++)
		{
			re = fftbuf[i];
			im = fftbuf[FFT_SIZE - i];
			fftPower[i] = re * re + im * im;
		}
	}
	*numFrame = nFrame;
	return	powerFFT;
}

void Tonality::WhitenSpectrumWithEnvelop(float *fftPower, int nFrame)
{
	float	*fft;
	int		frame, i;
	float	envelop[HALF_FFT];

	for (frame = 0; frame < nFrame; frame++)
	{
		if (_bUtter[frame]==false)	continue;
		fft = &fftPower[frame*HALF_FFT];
		GetSpectralEnvelop(fft, HALF_FFT, envelop);
		for (i = 0; i < HALF_FFT; i++)
		{
			fft[i] /= envelop[i];
		}
	}
}

void Tonality::GetSpectralEnvelop(float *fft, int num, float *envelop)
{
	int		range;
	int		i, j;
	bool	bPeak;
	int		last;
	float	E;

	range = (int)(EnvelopRange/_bandW);

	// search spectrum peaks
	assert(num > range);
	for (i = 0; i < num; i++)
	{
		bPeak = true;
		envelop[i] = -1;
		for (j = i - range; j < i + range && j != i; j++)
		{
			if (j < 0 || j >= num)
				continue;
			if (fft[j] > fft[i])
			{
				bPeak = false;
				break;
			}
		}
		if (bPeak)
			envelop[i] = (float)log(fft[i]+1);
	}
	// concatenate peaks by linear interpolation at log scale
	last = -1;
	for (i = 0; i < num; i++)
	{
		if (envelop[i] < 0)
			continue;
		if (last == -1)
		{
			for (j = 0; j < i; j++)	envelop[j] = envelop[i];
		}
		else
		{
			for (j = last+1; j < i; j++)
				envelop[j] = envelop[last] + (envelop[i] - envelop[last]) / (i-last) * (j-last);
		}
		last = i;
	}
	
	E = (last > 0) ? envelop[last] : 1;

	for (j = last+1; j < num; j++)
		envelop[j] = E;

	// reverse back to linear scale
	for (i = 0; i < num; i++)
	{
		assert(envelop[i] > 0);
		envelop[i] = (float)exp(envelop[i]);
	}
}

void Tonality::InitFBank(float lowPass,float highPass)
{
	float	mlow,mhigh;
	int		kLow, kHigh;
	float	mwidth;
	int		i,k;
	float	gama;
	float	fres;
	float	bnkCentre[BN+1];
	
	gama = 1;
	fres = TGT_SAMPLERATE / (FFT_SIZE*700.0F);
	kLow = 2; mlow = 0;
	if (lowPass >= 0.0F)
	{
		mlow = 1127 * (float)log(1.0+lowPass/700.0);
		kLow = (int)(lowPass*FFT_SIZE/(float)TGT_SAMPLERATE+0.5);
		if (kLow<2) kLow=2;
	}
	kHigh = HALF_FFT;
	mhigh = Mel(HALF_FFT, fres);
	if (highPass >= 0.0F)
	{
		mhigh = 1127*(float)log(1.0+highPass/700.0);
		kHigh = (int)(highPass*FFT_SIZE/(float)TGT_SAMPLERATE+0.5);
		if (kHigh>HALF_FFT) kHigh=HALF_FFT;
	}
	mwidth=mhigh-mlow;
	for (i=0;i<=BN;i++)
		bnkCentre[i]=((float)(i+1)/(float)(BN+1))*mwidth+mlow;
	
	fres*=gama;
	for (i=k=0;k<HALF_FFT;k++)
	{
		float mel=Mel(k, fres);
		while (bnkCentre[i]<mel && i<=BN) i++;
		_chlMap[k]=i-1;
	}
	for (k=0;k<HALF_FFT;k++) {
		i=_chlMap[k];
		if (i>=0) {
			float bnki1=bnkCentre[i+1];
			_chlWeight[k]=(bnki1-Mel(k, fres))/(bnki1-bnkCentre[i]);
		} else {
			float bnk0=bnkCentre[0];
			_chlWeight[k]=(bnk0-Mel(k, fres))/(bnk0-mlow);
		}
	}
}